#include <iostream>
#include <iomanip>
#include <vector>
#include <string>
#include <fstream>
#include <opencv2/opencv.hpp>

#define RESET   "\033[0m"
#define BLACK   "\033[30m"      /* Black */
#define RED     "\033[31m"      /* Red */
#define GREEN   "\033[32m"      /* Green */
#define YELLOW  "\033[33m"      /* Yellow */
#define BLUE    "\033[34m"      /* Blue */
#define MAGENTA "\033[35m"      /* Magenta */
#define CYAN    "\033[36m"      /* Cyan */
#define WHITE   "\033[37m"      /* White */
#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */

using namespace cv;
using namespace std;

#define HEIGHT 640

class ResultData
{
public:
    ResultData():timeStamp(-1.f),resultX(0.f),resultY(0.f),resultZ(0.f),
        diffX(0.f),diffY(0.f),diffZ(0.f),gpsTime(0.f)
    {}
    ResultData(double time, double x, double y, double z, double dx, double dy, double dz)
        :timeStamp(time),resultX(x),resultY(y),resultZ(z),
          diffX(dx),diffY(dy),diffZ(dz),gpsTime(0.f)
    {}

    friend ofstream& operator <<(ofstream& os, ResultData &data)
    {
        os << data.timeStamp << data.resultX << " " << data.resultY << " " << data.resultZ
           << " " << data.diffX << " " << data.diffY << " " << data.diffZ << " " << data.gpsTime << "\n";
        return os;
    }
    friend void operator >>(ifstream& os, ResultData &data)
    {
        double x,y,z;
        os >> data.timeStamp >> data.resultX >> data.resultY >> data.resultZ
                >> x >> y >> z >> data.gpsTime;
         data.diffX = x - data.resultX;
         data.diffY = y - data.resultY;
         data.diffZ = z - data.resultZ;
    }

public:
    double timeStamp;
    double resultX,resultY,resultZ;
    double diffX,diffY,diffZ;
    double gpsTime;
};

int addLine(cv::Mat& diffXMat,cv::Mat& diffYMat,cv::Mat& diffZMat,double maxDX,double maxDY,double maxDZ,double rateX,double rateY,double rateZ,double x)
{
    cv::Point2f lastX,lastY,lastZ;
    lastX.x = 0, lastX.y = (x+maxDX)*rateX;
    lastY.x = 0, lastY.y = (x+maxDY)*rateY;
    lastZ.x = 0, lastZ.y = (x+maxDZ)*rateZ;
    cv::Point2f midX(diffXMat.cols-1, (x+maxDX)*rateX),midY(diffYMat.cols-1,(x+maxDY)*rateY),midZ(diffZMat.cols-1,(x+maxDZ)*rateZ);
    cv::line(diffXMat,lastX,midX,Scalar(0));
    cv::line(diffYMat,lastY,midY,Scalar(0));
    cv::line(diffZMat,lastZ,midZ,Scalar(0));
    lastX.y = (-x+maxDX)*rateX;     lastY.y = (-x+maxDY)*rateY;     lastZ.y = (-x+maxDZ)*rateZ;
    midX.y = (-x+maxDX)*rateX;      midY.y = (-x+maxDY)*rateY;      midZ.y = (-x+maxDZ)*rateZ;
    cv::line(diffXMat,lastX,midX,Scalar(0,0,0));
    cv::line(diffYMat,lastY,midY,Scalar(0,0,0));
    cv::line(diffZMat,lastZ,midZ,Scalar(0,0,0));
}


int main(int argc, char *argv[])
{
    ifstream ifs;
    if(argc>1)
        ifs.open(argv[1]);
    else
        ifs.open("result.txt");

    if(!ifs.is_open())
    {
        cerr << "file open failed!\n";
        exit(-1);
    }

    ResultData data;
    vector<ResultData> result;

    double minX = 3000000, minY = 3000000, minZ = 3000000, maxX = -3000000, maxY = -3000000, maxZ = -3000000;
    double minDX = 100, minDY = 100, minDZ = 100, maxDX = -300, maxDY = -300, maxDZ = -300;
    double diffX = 0, diffY = 0, diffZ = 0;
    while (!ifs.eof()) {
        ifs >> data;
        if(fabs(data.diffY)>100)
        {
            cout << "dy error: " << fabs(data.diffY) << ", in: " << result.size() << endl;
            continue;
        }
        if(data.resultX > maxX) maxX = data.resultX;
        if(data.resultY > maxY) maxY = data.resultY;
        if(data.resultZ > maxZ) maxZ = data.resultZ;
        if(data.resultX < minX) minX = data.resultX;
        if(data.resultY < minY) minY = data.resultY;
        if(data.resultZ < minZ) minZ = data.resultZ;

        if(fabs(data.diffX) > maxDX) maxDX = fabs(data.diffX);
        if(fabs(data.diffY) > maxDY) maxDY = fabs(data.diffY);
        if(fabs(data.diffZ) > maxDZ) maxDZ = fabs(data.diffZ);
        if(fabs(data.diffX) < minDX) minDX = fabs(data.diffX);
        if(fabs(data.diffY) < minDY) minDY = fabs(data.diffY);
        if(fabs(data.diffZ) < minDZ) minDZ = fabs(data.diffZ);
        result.push_back(data);
        diffX = maxDX - minDX;
        diffY = maxDY - minDY;
        diffZ = maxDZ - minDZ;
    }
    cout << std::setiosflags(std::ios::fixed) << std::setprecision(6) << "minX = " << minX << "\nminY = " << minY << "\nminZ = " << minZ << "\nmaxX = " << maxX << "\nmaxY = " << maxY << "\nmaxZ = " << maxZ << endl;
    cout << "minDX = " << minDX << "\nminDY = " << minDY << "\nminDZ = " << minDZ << "\nmaxDX = " << maxDX << "\nmaxDY = " << maxDY << "\nmaxDZ = " << maxDZ << endl;


    double rateX = HEIGHT/diffX; rateX *= 0.5;
    double rateY = HEIGHT/diffY; rateY *= 0.5;
    double rateZ = HEIGHT/diffZ; rateZ *= 0.5;

    Mat diffXMat(HEIGHT,940*20,CV_8UC3,Scalar(255,255,255));
    Mat diffYMat(HEIGHT,940*20,CV_8UC3,Scalar(255,255,255));
    Mat diffZMat(HEIGHT,940*20,CV_8UC3,Scalar(255,255,255));

    cv::Point2f lastX, lastY, lastZ, lastTime, curTime, lastDiffTime, curDiffTime;
    for (size_t i = 0; i < result.size(); ++i) {
        float last_x = (result[i].timeStamp - result[0].timeStamp)*20;
        float last_y_x = (result[i].diffX + maxDX)*rateX;
        float last_y_y = (result[i].diffY + maxDY)*rateY;
        float last_y_z = (result[i].diffZ + maxDZ)*rateZ;
        cv::Point2f curX(last_x,last_y_x),curY(last_x,last_y_y),curZ(last_x,last_y_z);
        curDiffTime.x = last_x;
        curDiffTime.y = (result[i].timeStamp - result[i].gpsTime)*1000+300;
        if(i>0) {
            curTime.x = last_x;
            curTime.y = (result[i].timeStamp - result[i-1].timeStamp)*1000+300;
        }
        cv::line(diffXMat,lastTime,curTime,Scalar(255,255,0));
        cv::line(diffYMat,lastDiffTime,curDiffTime,Scalar(255,155,0));

        cv::line(diffXMat,lastX,curX,Scalar(255,0,0));
        cv::line(diffYMat,lastY,curY,Scalar(0,255,0));
        cv::line(diffZMat,lastZ,curZ,Scalar(0,0,255));
        lastX = curX, lastY = curY, lastZ = curZ, lastTime = curTime, lastDiffTime=curDiffTime;
    }
    for (int i = 1; i < 18; ++i) {
        cv::line(diffXMat,cv::Point2f(50*i*20,0),cv::Point2f(50*i*20,HEIGHT),Scalar(0,0,255),1);
        cv::line(diffYMat,cv::Point2f(50*i*20,0),cv::Point2f(50*i*20,HEIGHT),Scalar(0,0,255),1);
        cv::putText(diffXMat,std::to_string(50*i)+" s",cv::Point2f(50*i*20,80),4,2.5,Scalar( 0, 0, 0 ), 4);
        cv::putText(diffYMat,std::to_string(50*i)+" s",cv::Point2f(50*i*20,80),4,2.5,Scalar( 0, 0, 0 ), 4);
    }

    addLine(diffXMat,diffYMat,diffZMat,maxDX,maxDY,maxDZ,rateX,rateY,rateZ,0);
    addLine(diffXMat,diffYMat,diffZMat,maxDX,maxDY,maxDZ,rateX,rateY,rateZ,0.2);

    cv::imshow("X",diffXMat);
    cv::imshow("Y",diffYMat);
    cv::imshow("Z",diffZMat);
    cv::waitKey();
    cv::imwrite("X.png",diffXMat);
    cv::imwrite("Y.png",diffYMat);
    cv::imwrite("Z.png",diffZMat);
    return 0;
}



